import serial
import time

class SerialCommunication:
    def __init__(self, port='/dev/ttyUSB0', baud_rate=115200, timeout=1):
        self.ser = serial.Serial(
            port=port,
            baudrate=baud_rate,
            bytesize=8,
            parity='N',
            stopbits=1,
            timeout=timeout
        )
        self.sleep_time=0.01

    def send_request(self, request):
        self.ser.write(request)
        time.sleep(self.sleep_time) 
        response = self.ser.readline(self.ser.in_waiting)
        return response

class SingleMotor:
    def __init__(self, motor_id=1,serial_communication=None,motor_type=0):
        self.motor_id = motor_id
        self.serial_comm = serial_communication
        # 使能缓存
        self.enable_cache=1
        # 电机速度缓存
        self.speed_cache=1
        # 电机位置缓存
        self.position_cache=1
        
        self.montor_enable(0)
        
        if motor_type==0:
            self.motor_speed_type()
        else:
            self.motor_position_type()
            

    def calculate_crc(self, data: bytes):
        crc = 0xFFFF
        for pos in data:
            crc ^= pos
            for i in range(8):
                if (crc & 1) != 0:
                    crc >>= 1
                    crc ^= 0xA001
                else:
                    crc >>= 1
        crc = ((crc & 0xFF00) >> 8) | ((crc & 0x00FF) << 8)
        return crc.to_bytes(2, byteorder='big')

    # 传入参数：
    ## 功能码：0x06发送；0x03读取
    ## 寄存器地址
    ## 赋值/读取的寄存器数量
    def _create_request(self, function_code, start_address, data):
        request = self.motor_id.to_bytes(1, byteorder='big')
        request += function_code.to_bytes(1, byteorder='big')
        request += start_address.to_bytes(2, byteorder='big')
        request += data.to_bytes(2, byteorder='big',signed=True)
        request += self.calculate_crc(request)
        return request
    
    # 传入参数：
    ## 寄存器地址
    ## 赋值
    def _create_request_multiple(self, start_address,datas):
        num=len(datas)
        b_num=2*num
        function_code=0x10
        request = self.motor_id.to_bytes(1, byteorder='big')
        request += function_code.to_bytes(1, byteorder='big')
        request += start_address.to_bytes(2, byteorder='big')
        request += num.to_bytes(2, byteorder='big')
        request += b_num.to_bytes(1, byteorder='big')
        for data in datas:
            request += data.to_bytes(2, byteorder='big',signed=True)
        request += self.calculate_crc(request)
        return request
    
    # 32位寄存器控制
    def _create_request_32(self, start_address,data):
        num=2
        b_num=2*num
        function_code=0x10
        request = self.motor_id.to_bytes(1, byteorder='big')
        request += function_code.to_bytes(1, byteorder='big')
        request += start_address.to_bytes(2, byteorder='big')
        request += num.to_bytes(2, byteorder='big')
        request += b_num.to_bytes(1, byteorder='big')
        data_byte = data.to_bytes(4, byteorder='big', signed=True)
        swapped_data_byte = data_byte[2:] + data_byte[:2]
        request += swapped_data_byte
        request += self.calculate_crc(request)
        return request

    # 电机速度设置
    def motor_speed_set(self, motor_speed):
        if motor_speed!=self.speed_cache:
            self.speed_cache=motor_speed
            request = self._create_request(0x06, 0x0603, motor_speed)
            self.serial_comm.send_request(request)
        

    # 电机速度环设置
    def montor_speed_kp(self, kp):
        request = self._create_request(0x06, 0x0800, kp)
        self.serial_comm.send_request(request)

    # 电机使能
    def montor_enable(self, enable):
        if enable!=self.enable_cache:
            self.enable_cache=enable
            request = self._create_request(0x06, 0x0303, enable)
            print(self.serial_comm.send_request(request))

    # 电机速度获得
    def motor_speed_get(self):
        num = 1
        request = self._create_request(0x03, 0x0B00, num)
        response = self.serial_comm.send_request(request)
        decimal_value = int.from_bytes(response[3:5], byteorder='big',signed=True) if response else None
        decimal_value
    
    # 电机位置获得
    def motor_position_get(self):
        num = 2
        request = self._create_request(0x03, 0x0B0F, num)
        response = self.serial_comm.send_request(request)
        decimal_value = int.from_bytes(response[3:5], byteorder='big',signed=True) if response else None
        decimal_value
    
    # 电机位置控制
    def motor_position_set(self,position):
        if position!=self.position_cache:
            self.position_cache=position
            request = self._create_request(0x06, 0x0305, 0)
            self.serial_comm.send_request(request)
            request = self._create_request_32(0x110C,position)
            self.serial_comm.send_request(request)
            request = self._create_request(0x06, 0x0305, 1)
            self.serial_comm.send_request(request)
            # return self.serial_comm.send_request(request)


    # 速度模式设置
    def motor_speed_type(self):
        request = self._create_request_multiple(0x200,[0])
        self.serial_comm.send_request(request)
    
    # 位置模式设置
    def motor_position_type(self):
        request = self._create_request_multiple(0x200,[1])
        self.serial_comm.send_request(request)
   